#include <Servo.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <EEPROM.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <Wire.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include "Adafruit_NeoPixel.h"

//************************const variable for hardware*********************************//

#define CURRENT_DETECT_PORT1        A2
#define CURRENT_DETECT_PORT2        A1
#define BOT_VOLTAGE_PORT            A6
#define POWER_OFF_PIN               7//not defined
#define BOT_PUMP_PORT               3
#define BOT_MOTOR_M1                5
#define BOT_MOTOR_M2                9

#define GET 1
#define RUN 2
#define RESET 4
#define START 5

//Command

#define BOT_FORWARD                  0xF1
#define BOT_TURNRIGHT                0xF2
#define BOT_TURNLEFT                 0xF3
#define BOT_PUMP                     0xF4
#define BOT_POWER_OFF                0xF5
#define BOT_CURRENT_M1               0xF6
#define BOT_CURRENT_M2               0xF7
#define BOT_VOLTAGE                  0xF8
#define BOT_GPS                      0xF9

//******************** filter variable******************//
float K_P[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
float varP[6] = {pow(0.1, 2), pow(0.1, 2), pow(0.1, 2), pow(0.1, 2), pow(0.1, 2), pow(0.01, 2)};//回归速率 改大了就会快速回归，变小就会缓慢回归（滞后性）
float varM[6] = {pow(0.1, 2), pow(0.1, 2), pow(0.1, 2), pow(0.1, 2), pow(0.1, 2), pow(0.1, 2)};
float K[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
float Kalmanq[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 20.0};
//******************** filter variable******************//
//******************variable for commands**********************//
typedef struct MeModule
{
    int device;
    int port;
    int slot;
    int pin;
    int index;
    float values[3];
}
MeModule;

union
{
    byte byteVal[4];
    float floatVal;
    long longVal;
}
val;

union
{
    byte byteVal[8];
    double doubleVal;
}
valDouble;

union
{
    byte byteVal[2];
    short shortVal;
}
valShort;

int analogs[8] =
{
    A0, A1, A2, A3, A4, A5, A6, A7
};

//2-5;1-2;3-4;4-6;6-3;5-1;

String mVersion = "1.0.0";
boolean isAvailable = false;

int len = 52;
char buffer[52];
byte index = 0;
byte dataLen;
byte modulesLen = 0;
boolean isStart = false;
unsigned char irRead;
char serialRead;

unsigned char prevc = 0;
double lastTime = 0.0;
double currentTime = 0.0;
//******************variable for commands**********************//

int fx_vriable = 0;
uint8_t fx_idx ;

//*******************functions of protocol********************//
uint8_t readBuffer(int index)
{
    return buffer[index];
}

void writeBuffer(int index, unsigned char c)
{
    buffer[index] = c;
}
void writeHead(uint8_t mode)
{
    writeSerial(0xf0);
    writeSerial(0x55);
    writeSerial(mode);
}
void writeEnd()
{
    Serial.println();
}
void writeSerial(unsigned char c)
{
    Serial.write(c);
}
void readSerial()
{
    isAvailable = false;
    if (Serial.available() > 0) ////////////////////////////////////////////////////////maybe cause problems
    {
        isAvailable = true;
        serialRead = Serial.read();

    }
}


void parseData()
{

    isStart = false;
    int idx = readBuffer(3);
    int action = readBuffer(4);
    int device = readBuffer(5);

    switch (action)
    {
    case GET:
    {
        writeHead(0x01);
        writeSerial(idx);
        readSensor(device);
        writeEnd();
    }
    break;
    case RUN:
    {
        runModule(device);
        callOK(idx, 0x10);
    }
    break;
    case RESET:
    {
        SystemReset();
        callOK(idx, 0x10);
    }
    break;
    case START:
    {
        callOK(idx, 0x10);
    }
    break;
    }
}
void SystemReset()
{

}
void callOK(uint8_t index, uint8_t errorcode)
{
    writeSerial(0xf0);
    writeSerial(0x55);
    writeSerial(0x01);
    writeSerial(index);
    writeSerial(errorcode);
    writeEnd();
}
void sendByte(char c)
{
    writeSerial(0x01);
    writeSerial(c);
}
void sendString(String s)
{
    int l = s.length();
    writeSerial(0x04);
    writeSerial(l);
    for (int i = 0; i < l; i++)
    {
        writeSerial(s.charAt(i));
    }
}
void sendFloat(float value)
{
    writeSerial(0x2);
    val.floatVal = value;
    writeSerial(val.byteVal[0]);
    writeSerial(val.byteVal[1]);
    writeSerial(val.byteVal[2]);
    writeSerial(val.byteVal[3]);
}
void sendShort(double value)
{
    writeSerial(0x03);
    valShort.shortVal = value;
    writeSerial(valShort.byteVal[0]);
    writeSerial(valShort.byteVal[1]);
}
void sendDouble(double value)
{
    writeSerial(0x05);
    valDouble.doubleVal = value;
    writeSerial(valDouble.byteVal[0]);
    writeSerial(valDouble.byteVal[1]);
    writeSerial(valDouble.byteVal[2]);
    writeSerial(valDouble.byteVal[3]);
}
void sendAngle(int Pitch, int Yaw, int Roll)
{
    writeSerial(0x22);
    valShort.shortVal = Pitch;
    writeSerial(valShort.byteVal[0]);
    writeSerial(valShort.byteVal[1]);
    valShort.shortVal = Yaw;
    writeSerial(valShort.byteVal[0]);
    writeSerial(valShort.byteVal[1]);
    valShort.shortVal = Roll;
    writeSerial(valShort.byteVal[0]);
    writeSerial(valShort.byteVal[1]);
}
short readShort(int idx)
{
    valShort.byteVal[0] = readBuffer(idx);
    valShort.byteVal[1] = readBuffer(idx + 1);
    return valShort.shortVal;
}
float readFloat(int idx)
{
    val.byteVal[0] = readBuffer(idx);
    val.byteVal[1] = readBuffer(idx + 1);
    val.byteVal[2] = readBuffer(idx + 2);
    val.byteVal[3] = readBuffer(idx + 3);
    return val.floatVal;
}

void runModule(int device)
{
    switch (device)
    {
    case BOT_FORWARD:
    {

        uint8_t speed = readBuffer(6);
        RunMotor(BOT_FORWARD,speed);
        callOK(fx_idx, 0x10);
    }
    break;
    case BOT_TURNLEFT:
    {
        uint8_t speed = readBuffer(6);
        RunMotor(BOT_TURNLEFT,speed);
        callOK(fx_idx, 0x10);
    }
    break;
    case BOT_TURNRIGHT:///////////////////////////////////////////////////////////////////////////////////set effect
    {
        uint8_t speed = readBuffer(6);
        RunMotor(BOT_TURNRIGHT,speed);
        callOK(fx_idx, 0x10);//////////////////////////////////////////
    }
    break;
    case BOT_PUMP:
    {
        uint8_t speed = readBuffer(6);
        PumpGo(speed);
        callOK(fx_idx, 0x10);//////////////////////////////////////////

    }
    break;
    case BOT_POWER_OFF:
    {
        digitalWrite(POWER_OFF_PIN, HIGH);
    }
    break;
    case BOT_VOLTAGE:
    {
        int dc_value = analogRead(BOT_VOLTAGE_PORT);
        float voltage_val = (dc_value / 1024.0) * 1.1 * 10.0 + 0.16;
        sendFloat(voltage_val);
    }
    break;
    case BOT_CURRENT_M1:
    {
        int dc_current = analogRead(CURRENT_DETECT_PORT1);
        byte current = caculate_Current(dc_current);
        sendByte(current);
    }
    break;
    case BOT_CURRENT_M2:
    {
        int dc_current = analogRead(CURRENT_DETECT_PORT2);
        byte current = caculate_Current(dc_current);
        sendByte(current);
    }
    break;
    case BOT_GPS:
    {
        String position = get_GPS_Position();
        sendString(position);
    }
    break;
    default:
        break;
    }
}
void readSensor(int device)
{


}
String get_GPS_Position()
{
    String position;
    // add your code here to get GPS Position
    return position;
}
uint8_t caculate_Current(int pin_current)
{
 uint8_t Bot_Current = 0;
 Bot_Current = uint8_t(analogRead(pin_current)/15);   
 return Bot_Current;
}
void PumpGo(int speed)
{
    analogWrite(BOT_PUMP_PORT, speed);
}
void RunMotor(uint8_t direction,int speed)
{
    if(direction == BOT_FORWARD){
       analogWrite(BOT_MOTOR_M1, speed); 
       analogWrite(BOT_MOTOR_M2, speed); 
    }
    if(direction == BOT_TURNLEFT){
        analogWrite(BOT_MOTOR_M1, speed);
        analogWrite(BOT_MOTOR_M2, speed/2);
    }
    if(direction == BOT_TURNRIGHT){
        analogWrite(BOT_MOTOR_M2, speed);
        analogWrite(BOT_MOTOR_M1, speed/2);
    }
}

void read_serial(void)

{
    while (isAvailable)
    {
        // deviceState = 1;
        unsigned char c = serialRead & 0xff;
        if ((c == 0x55) && (isStart == false))
        {
            if (prevc == 0xf0)
            {
                index = 1;
                isStart = true;
            }
        }
        else
        {
            prevc = c;
            if (isStart)
            {
                if (index == 2)
                {
                    dataLen = c;
                }
                else if (index > 2)
                {
                    dataLen--;
                }
                // cleanBuffer();
                writeBuffer(index, c);
            }
        }
        index++;
        if (index > 51)
        {
            index = 0;
            isStart = false;
        }
        if (isStart && (dataLen == 0) && (index > 3))
        {
            isStart = false;
            parseData();
            index = 0;
        }
        readSerial();
    }
}

void try_to_power_off()
{

}


float Kalman(float inByte, int flag)
{
    K_P[flag] = K_P[flag] + varP[flag];
    K[flag] = K_P[flag] / (K_P[flag] + varM[flag]);
    Kalmanq[flag] = K[flag] * inByte + (1 - K[flag]) * Kalmanq[flag];
    K_P[flag] = (1 - K[flag]) * K_P[flag];
    return Kalmanq[flag];
}


void setup()
{
    Serial.begin(9600);  
    Serial.print("Version: ");
    Serial.println(mVersion);
    lastTime = millis();
}
void loop()
{
    readSerial();
    if (isAvailable)
    {
        read_serial();
    }
 
}
